/**
 * klasse fer die ki steuerung des koffers
 * 
 * wandelt die sensordaten in steuerungsbefehle um
 * 
 */
package ch.unizh.ifi.ailab.ai;

import java.util.Iterator;

import org.apache.log4j.Logger;

import ch.unizh.ifi.ailab.config.ProgramSettings;
import ch.unizh.ifi.ailab.dataTypes.Command;
import ch.unizh.ifi.ailab.dataTypes.Fifo;
import ch.unizh.ifi.ailab.dataTypes.SensorData;


/**
 * @author hans jaeckle jaeckle@gmail.com
 * 
 */

public class AICore {

	/**
	 * enthaelt die eine liste mit den berechneten durchschnittswerten dient zur
	 * speicherung der mittelwerte ueber die zeit inhalt: Null15SensorData
	 */
	private Fifo artihmeticMeanFifo = null;

	/**
	 * enthaelt die eine liste mit den berechneten durchschnittswerten dient zur
	 * speicherung der mittelwerte ueber die zeit inhalt: Null15SensorData
	 */
	private Fifo geometricMeanFifo = null;

	/**
	 * enthaelt die in den properties definierte anzahl an sensordatensaetzen
	 * dient zur berechnung der mittelwerte ueber die zeit. inhalt:
	 * Null15SensorData
	 */
	private Fifo dataFifo = null;

	/**
	 * enthaelt die in den properties definierte anzahl an sensordatensaetzen
	 * dient zur berechnung der mittelwerte ueber die zeit. inhalt:
	 * Null15SensorData
	 */
	private Fifo commandFifo = null;

	// smcType enthaelt den typ des zu verwendenden SMC
	private SensorMotorCoupling mySMC = null;

	// TODO: SMC verwaltung
	// private String defaultType = "vectorSMC";

	// properties
	private ProgramSettings myProp = ProgramSettings.getProgramSettings();

	// logging
	private Logger myLog = Logger.getLogger(AICore.class);

	/**
	 * konstruktor methode zum initialisieren der noetigen speicherelemente.
	 */
	public AICore() {
		myLog.debug("ki: initialisiere core.");
		// fiifo zum speichern einer sensordatenreihe
		artihmeticMeanFifo = new Fifo(myProp.getIntProperty("kiMeanFifoLength"));
		geometricMeanFifo = new Fifo(myProp.getIntProperty("kiMeanFifoLength"));
		dataFifo = new Fifo(myProp.getIntProperty("kiDataFifoLength"));
		commandFifo = new Fifo(myProp.getIntProperty("kiCommandFifoLength"));

		// initialisiere SMC
		switchSMC(null);
	}

	/**
	 * calculatenextcommand methode zum zugriff auf die ki. das uebergebene
	 * command und die daten werden in die ensprechenden speicher geschoben und
	 * die berechnung des n�chsten kommandos wird gestartet. die berechnung
	 * findet auf dem �bergebenen kommando statt.
	 * 
	 * @param data
	 *            datensatz mit der aktuellen sensorwerten
	 * @param command
	 *            aktuelles kommando
	 */
	public void calculateNextComand(SensorData data, Command command)
			throws UnavoidableObjectException {
		dataFifo.pushback(data);
		commandFifo.pushback(command);
		artihmeticMeanFifo.pushback(arithmeticMean(dataFifo));
		geometricMeanFifo.pushback(geometricMean(dataFifo));

		// processData(command);
		mySMC.calculateMotionCommand(data, command);
	}

	/**
	 * switchSMC methode zum �ndern des zu verwendenden SMC algorythmus.
	 * 
	 * @param smcType
	 *            noch unwirksam, da nur ein SMC typ existiert.
	 */
	public void switchSMC(String smcType) {
		// TODO: SMC Typen in die Properties nehmen und von dort auslesen.
		if (smcType == null) {
			mySMC = new SMCSquareRepulsive();
		} else if (smcType.equals("vectorSMC")) {
			mySMC = new SMCVector();
		} else {
			mySMC = new SMCRangeHistogram();
		}
	}

	public String getKISMC() {
		return mySMC.getSMCType();
	}

	public Fifo getArtihmeticMeanFifo() {
		return artihmeticMeanFifo;
	}

	public Fifo getCommandFifo() {
		return commandFifo;
	}

	public Fifo getDataFifo() {
		return dataFifo;
	}

	public Fifo getGeometricMeanFifo() {
		return geometricMeanFifo;
	}

	/**
	 * arithmeticmean methode zum berechnen des arithmetischen mittels �ber die
	 * in der datafifo abgelegten datens�tze.
	 * 
	 * @param f
	 *            null15fifo mit daten der gemittelt werden soll
	 * @return ist ein null15datensatz mit den gemittelten werten aus dem fifo.
	 */
	private SensorData arithmeticMean(Fifo f) {
		SensorData meanData = new SensorData();

		Iterator i = f.iterator();
		SensorData nextData;
		
		while (i.hasNext()) {
			nextData = (SensorData) i.next();
			meanData.setIr1(meanData.getIr1() + nextData.getIr1());
			meanData.setIr2(meanData.getIr2() + nextData.getIr2());
			meanData.setIr3(meanData.getIr3() + nextData.getIr3());
			meanData.setIr4(meanData.getIr4() + nextData.getIr4());

			meanData.setIo1(meanData.getIo1() + nextData.getIo1());
			meanData.setIo2(meanData.getIo2() + nextData.getIo2());
			meanData.setIo3(meanData.getIo3() + nextData.getIo3());
			meanData.setIo4(meanData.getIo4() + nextData.getIo4());

			meanData.setUs1(meanData.getUs1() + nextData.getUs1());
			meanData.setUs2(meanData.getUs2() + nextData.getUs2());
			meanData.setUs3(meanData.getUs3() + nextData.getUs3());
			meanData.setUs4(meanData.getUs4() + nextData.getUs4());
		}
		meanData.setIr1(meanData.getIr1() / f.getLength());
		meanData.setIr2(meanData.getIr2() / f.getLength());
		meanData.setIr3(meanData.getIr3() / f.getLength());
		meanData.setIr4(meanData.getIr4() / f.getLength());

		meanData.setIo1(meanData.getIo1() / f.getLength());
		meanData.setIo2(meanData.getIo2() / f.getLength());
		meanData.setIo3(meanData.getIo3() / f.getLength());
		meanData.setIo4(meanData.getIo4() / f.getLength());

		meanData.setUs1(meanData.getUs1() / f.getLength());
		meanData.setUs2(meanData.getUs2() / f.getLength());
		meanData.setUs3(meanData.getUs3() / f.getLength());
		meanData.setUs4(meanData.getUs4() / f.getLength());

		// myLog.debug("ki: arithmetisches mittel fuer aktuelle sensordaten = "
		// + meanData.toString());

		return meanData;
	}

	/**
	 * geometric methode zum berechnen des geometrischen mittels �ber die in der
	 * datafifo abgelegten datens�tze.
	 * 
	 * @param f
	 *            null15fifo mit daten der gemittelt werden soll
	 * @return ist ein null15datensatz mit den gemittelten werten aus dem fifo.
	 */
	private SensorData geometricMean(Fifo f) {
		SensorData meanData = new SensorData();

		Iterator i = f.iterator();
		SensorData nextData;
		// berechnung des mittels:
		while (i.hasNext()) {
			nextData = (SensorData) i.next();
			meanData.setIr1(meanData.getIr1() * nextData.getIr1());
			meanData.setIr2(meanData.getIr2() * nextData.getIr2());
			meanData.setIr3(meanData.getIr3() * nextData.getIr3());
			meanData.setIr4(meanData.getIr4() * nextData.getIr4());

			meanData.setIo1(meanData.getIo1() * nextData.getIo1());
			meanData.setIo2(meanData.getIo2() * nextData.getIo2());
			meanData.setIo3(meanData.getIo3() * nextData.getIo3());
			meanData.setIo4(meanData.getIo4() * nextData.getIo4());

			meanData.setUs1(meanData.getUs1() * nextData.getUs1());
			meanData.setUs2(meanData.getUs2() * nextData.getUs2());
			meanData.setUs3(meanData.getUs3() * nextData.getUs3());
			meanData.setUs4(meanData.getUs4() * nextData.getUs4());
		}
		meanData.setIr1(meanData.getIr1() ^ (1 / f.getLength()));
		meanData.setIr2(meanData.getIr2() ^ (1 / f.getLength()));
		meanData.setIr3(meanData.getIr3() ^ (1 / f.getLength()));
		meanData.setIr4(meanData.getIr4() ^ (1 / f.getLength()));

		meanData.setIo1(meanData.getIo1() ^ (1 / f.getLength()));
		meanData.setIo2(meanData.getIo2() ^ (1 / f.getLength()));
		meanData.setIo3(meanData.getIo3() ^ (1 / f.getLength()));
		meanData.setIo4(meanData.getIo4() ^ (1 / f.getLength()));

		meanData.setUs1(meanData.getUs1() ^ (1 / f.getLength()));
		meanData.setUs2(meanData.getUs2() ^ (1 / f.getLength()));
		meanData.setUs3(meanData.getUs3() ^ (1 / f.getLength()));
		meanData.setUs4(meanData.getUs4() ^ (1 / f.getLength()));

		// myLog.debug("ki: geometrisches mittel f�r aktuelle sensordaten = " +
		// meanData.toString());

		return meanData;
	}
}
